// #include <gtest/gtest.h>
// #include <gmock/gmock.h>
// #include "axisgroup/axisgroup.h"
// #include "axisgroup/pfsm_servo.h"
// #include "axis/axis.h"

// #include <thread>
// #include <sys/shm.h>
// #include <sys/sem.h>

// #define TEST_COUNT 5
// #define SERVOJ_SEM_KEY 2002

// using namespace axisgroup;

// volatile sig_atomic_t should_exit = 0;

// static int sem_get(key_t key);
// static int sem_p(int sem_id);
// static int sem_v(int sem_id);

// class MockAxis : public axis::Axis
// {
// public:
//     MOCK_METHOD0(run_state, RunState());
//     MOCK_METHOD0(get_id, int());
// };

// void handle_sigint(int sig_num)
// {
//     should_exit = 1;
//     printf("Received SIGINT, setting exit flaxis_grp->\n");
// }
// class ServoUserTest : public ::testing::Test
// {
// protected:
//     virtual void SetUp() override
//     {
//         for (int i = 0; i < 7; i++) { EXPECT_CALL(joint[i], get_id()).WillRepeatedly(testing::Return(i)); }

//         struct sigaction sa;

//         // 设置SIGINT的信号处理函数
//         sa.sa_handler = handle_sigint;
//         sigemptyset(&sa.sa_mask);
//         sa.sa_flags = 0;
//         sigaction(SIGINT, &sa, NULL);

//         pid_ = fork();
//         if (pid_ < 0)
//         {
//             exit(1);
//         }
//         if (pid_ == 0)
//         {
//             printf("--------------Child---------------- %d\n", pid_);

//             ServoMoveShm* servo_move_shm_;
//             shm_id_ = shmget((key_t)SERVOJ_SEM_KEY, sizeof(ServoMoveShm), IPC_CREAT | 0666);
//             ASSERT_TRUE(shm_id_ > 0);
//             servo_move_shm_ = (ServoMoveShm*)shmat(shm_id_, NULL, 0);
//             ASSERT_TRUE(servo_move_shm_ != 0);

//             ServoMoveData user_data = {};
//             for (int i = 0; i < TEST_COUNT && !should_exit; i++)
//             {
//                 printf("TEST COUNT %d\n", i);
//                 user_data.type = 0;
//                 user_data.relFlag = 0;
//                 user_data.stepNum = 1;
//                 user_data.dirty = 1;
//                 for (int j = 0; j < 7; j++) { user_data.servojPos[j] = i; }
//                 if (servo_move_shm_)
//                 {
//                     int breakFlag = 0, waitTimes = 0, sem_id;
//                     struct timespec ts = {0, 1000000};  // 1ms
//                     for (waitTimes = 0; waitTimes < user_data.stepNum * 8 + 4; waitTimes++)
//                     {
//                         sem_id = sem_get(SERVOJ_SEM_KEY);
//                         if (sem_p(sem_id) == 0)
//                         {
//                             // printf("servojShmStruct->dirty = %d\n", servojShmStruct->dirty);
//                             if (!servo_move_shm_->dirty)
//                             {
//                                 servo_move_shm_->servoMoveData[0] = user_data;
//                                 servo_move_shm_->dirty = 1;
//                                 breakFlag = 1;
//                                 // printf("----------------------Child send pos %lf\n", servo_move_shm_->servoMoveData[0].servojPos[0]);
//                             }
//                             sem_v(sem_id);
//                         }
//                         if (breakFlag)
//                             break;
//                         nanosleep(&ts, NULL);
//                     }
//                     if (waitTimes >= user_data.stepNum * 8 + 4)
//                     {
//                         printf("-------------------Child Timeout!\n");
//                     }
//                 }
//                 usleep(100);
//             }

//             shmdt(servo_move_shm_);  // 使用shmdt函数将共享内存从当前进程的地址空间中分离出来。
//             printf("child over!\n");
//             exit(0);
//         }
//     }
//     virtual void TearDown() override {}

//     MockAxis joint[7];
//     pid_t pid_;
//     int shm_id_ = -1;
// };

// TEST_F(ServoUserTest, SERVO_MOVE)
// {
//     if (pid_)
//     {
//         printf("--------------parent---------------- %d\n", pid_);

//         std::shared_ptr<AxisGroup> axis_grp = std::make_shared<AxisGroup>();
//         axis_grp->reset();
//         axis_grp->set_group_id(0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[0]), 0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[1]), 0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[2]), 0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[3]), 0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[4]), 0);
//         ASSERT_EQ(axis_grp->link_joint(&joint[5]), 0);
//         ASSERT_EQ(axis_grp->joint_num(), 6);

//         auto servo = std::make_shared<PFsmServo>(axis_grp);
//         EXPECT_EQ(servo->enter_mode(), 0);
//         servo->set_filter_type(PFsmServo::FilterType::NONE);

//         bool start = 0;
//         int i = 0;
//         while (i < TEST_COUNT - 1)
//         {
//             printf("--------------parent process %d\n", i);
//             int has_new_data = servo->get_user_cmd();
//             ASSERT_EQ(servo->get_servo_que_size(), 1);
//             servo->update();
//             ASSERT_EQ(servo->get_servo_que_size(), 0);
//             AxisGroupPose res = axis_grp->get_cur_cmd_pos();
//             for (int j = 0; j < 1; j++) { EXPECT_DOUBLE_EQ(res.jpos.val[j], i); }
//             if (has_new_data)
//             {
//                 i++;
//             }
//             else
//             {
//                 printf("No data!\n");
//             }
//             // axis_grp->
//             usleep(10);
//         }
//     }
// }

// typedef union semun
// {
//     int val;
//     struct semid_ds* buf;
//     unsigned short* array;
// } semun;

// int sem_get(key_t key)
// {
//     int sem_id = semget(key, 1, 0666 | IPC_CREAT);
//     union semun sem_union;
//     sem_union.val = 1;
//     if (semctl(sem_id, 0, SETVAL, sem_union) == -1)
//     {
//         return -1;
//     }
//     return sem_id;
// }

// //obtain a semaphore
// int sem_p(int sem_id)
// {
//     struct sembuf sem_buf;
//     sem_buf.sem_op = -1;
//     sem_buf.sem_flg = SEM_UNDO;
//     sem_buf.sem_num = 0;
//     if (semop(sem_id, &sem_buf, 1) == -1)
//     {
//         return -1;
//     }
//     return 0;
// }

// //release an semaphore
// int sem_v(int sem_id)
// {
//     struct sembuf sem_buf;
//     sem_buf.sem_flg = SEM_UNDO;
//     sem_buf.sem_num = 0;
//     sem_buf.sem_op = 1;
//     if (semop(sem_id, &sem_buf, 1) == -1)
//     {
//         return -1;
//     }
//     return 0;
// }